// SLIC.cpp: implementation of the SLIC class.
//===========================================================================
// This code implements the zero parameter superpixel segmentation technique
// described in:
//
//
//
// "SLIC Superpixels Compared to State-of-the-art Superpixel Methods"
//
// Radhakrishna Achanta, Appu Shaji, Kevin Smith, Aurelien Lucchi, Pascal Fua,
// and Sabine Susstrunk,
//
// IEEE TPAMI, Volume 34, Issue 11, Pages 2274-2282, November 2012.
//
// https://www.epfl.ch/labs/ivrl/research/slic-superpixels/
//===========================================================================
// Copyright (c) 2013 Radhakrishna Achanta.
//
// For commercial use please contact the author:
//
// Email: firstname.lastname@epfl.ch
//===========================================================================

#include <stdio.h>
#include <cfloat>
#include <cmath>
#include <iostream>
#include <fstream>
#include "SLIC.h"
#include <chrono>
#include <cstring>
#include <x86intrin.h>
#include <omp.h>
// #include <Eigen/Dense>
// #include "../avx_mathfun/avx_mathfun.h"
#include "utils.hpp"

typedef chrono::high_resolution_clock Clock;
#define __TEST__
// For superpixels
const int dx4[4] = {-1,  0,  1,  0};
const int dy4[4] = { 0, -1,  0,  1};
//const int dx8[8] = {-1, -1,  0,  1, 1, 1, 0, -1};
//const int dy8[8] = { 0, -1, -1, -1, 0, 1, 1,  1};

// For supervoxels
const int dx10[10] = {-1,  0,  1,  0, -1,  1,  1, -1,  0, 0};
const int dy10[10] = { 0, -1,  0,  1, -1, -1,  1,  1,  0, 0};
const int dz10[10] = { 0,  0,  0,  0,  0,  0,  0,  0, -1, 1};

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

SLIC::SLIC()
{
	m_lvec = NULL;
	m_avec = NULL;
	m_bvec = NULL;

	m_lvecvec = NULL;
	m_avecvec = NULL;
	m_bvecvec = NULL;
}

SLIC::~SLIC()
{
	// if(m_lvec) delete [] m_lvec;
	if(m_lvec) align_delete(m_lvec);
	// if(m_avec) delete [] m_avec;
	if(m_avec) align_delete(m_avec);
	// if(m_bvec) delete [] m_bvec;
	if(m_bvec) align_delete(m_bvec);


	if(m_lvecvec)
	{
		for( int d = 0; d < m_depth; d++ ) delete [] m_lvecvec[d];
		delete [] m_lvecvec;
	}
	if(m_avecvec)
	{
		for( int d = 0; d < m_depth; d++ ) delete [] m_avecvec[d];
		delete [] m_avecvec;
	}
	if(m_bvecvec)
	{
		for( int d = 0; d < m_depth; d++ ) delete [] m_bvecvec[d];
		delete [] m_bvecvec;
	}
}

//==============================================================================
///	RGB2XYZ
///
/// sRGB (D65 illuninant assumption) to XYZ conversion
/// (BT709 RGB)
/// 可参考：https://www.jianshu.com/p/c34a313e12eb
//==============================================================================
void SLIC::RGB2XYZ(
	const int&		sR,
	const int&		sG,
	const int&		sB,
	double&			X,
	double&			Y,
	double&			Z)
{
	// double R = sR/255.0;
	// double G = sG/255.0;
	// double B = sB/255.0;

	double r, g, b;

	if(sR <= 10)	r = sR/3294.6;
	else			r = pow(sR/269.025+0.055/1.055,2.4);
	if(sG <= 10)	g = sG/3294.6;
	else			g = pow(sG/269.025+0.055/1.055,2.4);
	if(sB <= 10)	b = sB/3294.6;
	else			b = pow(sB/269.025+0.055/1.055,2.4);

	X = r*0.4124564 + g*0.3575761 + b*0.1804375;
	Y = r*0.2126729 + g*0.7151522 + b*0.0721750;
	Z = r*0.0193339 + g*0.1191920 + b*0.9503041;
}

//===========================================================================
///	RGB2LAB
/// 可参考：https://blog.csdn.net/lz0499/article/details/77345166
/// RGB 不能直接转到 LAB，需要借助 XYZ 颜色空间
//===========================================================================
void SLIC::RGB2LAB(const int& sR, const int& sG, const int& sB, double& lval, double& aval, double& bval)
{
	//------------------------
	// sRGB to XYZ conversion
	//------------------------
	double X, Y, Z;
	RGB2XYZ(sR, sG, sB, X, Y, Z);
	XYZ2LAB(X, Y, Z, lval, aval, bval);
	// //------------------------
	// // XYZ to LAB conversion
	// //------------------------
	// double epsilon = 0.008856;	//actual CIE standard
	// double kappa   = 903.3;		//actual CIE standard

	// double Xr = 0.950456;	//reference white
	// double Yr = 1.0;		//reference white
	// double Zr = 1.088754;	//reference white

	// double xr = X/Xr;
	// double yr = Y/Yr;
	// double zr = Z/Zr;

	// double fx, fy, fz;
	// if(xr > epsilon)	fx = pow(xr, 1.0/3.0);
	// else				fx = (kappa*xr + 16.0)/116.0;
	// if(yr > epsilon)	fy = pow(yr, 1.0/3.0);
	// else				fy = (kappa*yr + 16.0)/116.0;
	// if(zr > epsilon)	fz = pow(zr, 1.0/3.0);
	// else				fz = (kappa*zr + 16.0)/116.0;

	// lval = 116.0*fy-16.0;
	// aval = 500.0*(fx-fy);
	// bval = 200.0*(fy-fz);
}

void SLIC::XYZ2LAB(const double& X, const double& Y, const double& Z, double& lval, double& aval, double& bval)
{
	//------------------------
	// XYZ to LAB conversion
	//------------------------
	double epsilon = 0.008856;	//actual CIE standard
	double kappa   = 903.3;		//actual CIE standard

	double Xr = 0.950456;	//reference white
	double Yr = 1.0;		//reference white
	double Zr = 1.088754;	//reference white

	double xr = X/Xr;
	double yr = Y/Yr;
	double zr = Z/Zr;

	double fx, fy, fz;
	if(xr > epsilon)	fx = pow(xr, 1.0/3.0);
	else				fx = (kappa*xr + 16.0)/116.0;
	if(yr > epsilon)	fy = pow(yr, 1.0/3.0);
	else				fy = (kappa*yr + 16.0)/116.0;
	if(zr > epsilon)	fz = pow(zr, 1.0/3.0);
	else				fz = (kappa*zr + 16.0)/116.0;

	lval = 116.0*fy-16.0;
	aval = 500.0*(fx-fy);
	bval = 200.0*(fy-fz);
}

//===========================================================================
///	DoRGBtoLABConversion
///
///	For whole image: overlaoded floating point version
//===========================================================================
void SLIC::DoRGBtoLABConversion(
	const unsigned int*&		ubuff,
	double*&					lvec,
	double*&					avec,
	double*&					bvec)
{
	int sz = m_width*m_height;
	// lvec = new double[sz];
	lvec = align_new<double>(sz, 32);
	// avec = new double[sz];
	avec = align_new<double>(sz, 32);
	// bvec = new double[sz];
	bvec = align_new<double>(sz, 32);
	const double epsilon = 0.008856;	//actual CIE standard
	const double kappa   = 903.3;		//actual CIE standard

	const double Xr = 0.950456;	//reference white
	const double Yr = 1.0;		//reference white
	const double Zr = 1.088754;	//reference white
	
	const double rgb_xyz_cont[5] = {3294.6, 1/269.025, 0.055/1.055, 2.4, 255.0 * 0.04045};
	const double rgb_xyz_mat[9] = { 0.4124564, 0.3575761, 0.1804375,
									0.2126729, 0.7151522, 0.0721750,
									0.0193339, 0.1191920, 0.9503041};
	const double xyz_lab_cont[12] = {Xr, Yr, Zr, epsilon, 903.3/116.0, 1.0/3.0, 16.0/116.0, 
									116.0, 16.0, 500.0, 200.0, 0};
	__m256d mm_rgb_xyz_cont[5];
	__m256d mm_rgb_xyz_mat[9];
	__m256d mm_xyz_lab_cont[12];
	for(int i=0; i<5; i++)
		mm_rgb_xyz_cont[i] = _mm256_broadcast_sd(rgb_xyz_cont + i);
	for(int i=0; i<9; i++)
		mm_rgb_xyz_mat[i] = _mm256_broadcast_sd(rgb_xyz_mat + i);
	for(int i=0; i<12; i++)
		mm_xyz_lab_cont[i] = _mm256_broadcast_sd(xyz_lab_cont + i);

	int j1 = 0;
	// j1 = sz - sz % DOUBLE_AVX2;
	// #pragma omp parallel for //num_threads(8)
	// for(int j=0; j < sz-DOUBLE_AVX2; j+=DOUBLE_AVX2){
	// 	alignas(32) double block_r[DOUBLE_AVX2];
	// 	alignas(32) double block_g[DOUBLE_AVX2];
	// 	alignas(32) double block_b[DOUBLE_AVX2];
	// 	alignas(32) double block_x[DOUBLE_AVX2];
	// 	alignas(32) double block_y[DOUBLE_AVX2];
	// 	alignas(32) double block_z[DOUBLE_AVX2];
	// 	// auto startTime = Clock::now();
	// 	for(int cc=0; cc < DOUBLE_AVX2; cc++){
	// 		block_r[cc] = (ubuff[j+cc] >> 16) & 0xFF;
	// 		block_g[cc] = (ubuff[j+cc] >>  8) & 0xFF;
	// 		block_b[cc] = (ubuff[j+cc]      ) & 0xFF;
	// 	}
	// 	__m256d mm_r = _mm256_load_pd(block_r);
	// 	__m256d mm_g = _mm256_load_pd(block_g);
	// 	__m256d mm_b = _mm256_load_pd(block_b);
	// 	mm_r = _mm256_blendv_pd(_mm256_div_pd(mm_r, mm_rgb_xyz_cont[0]), 
	// 							_ZGVdN4vv_pow(_mm256_fmadd_pd(mm_r, mm_rgb_xyz_cont[1], mm_rgb_xyz_cont[2]), mm_rgb_xyz_cont[3]), 
	// 							_mm256_cmp_pd(mm_r, mm_rgb_xyz_cont[4], _CMP_GT_OQ));
	// 	mm_g = _mm256_blendv_pd(_mm256_div_pd(mm_g, mm_rgb_xyz_cont[0]), 
	// 							_ZGVdN4vv_pow(_mm256_fmadd_pd(mm_g, mm_rgb_xyz_cont[1], mm_rgb_xyz_cont[2]), mm_rgb_xyz_cont[3]), 
	// 							_mm256_cmp_pd(mm_g, mm_rgb_xyz_cont[4], _CMP_GT_OQ));
	// 	mm_b = _mm256_blendv_pd(_mm256_div_pd(mm_b, mm_rgb_xyz_cont[0]), 
	// 							_ZGVdN4vv_pow(_mm256_fmadd_pd(mm_b, mm_rgb_xyz_cont[1], mm_rgb_xyz_cont[2]), mm_rgb_xyz_cont[3]), 
	// 							_mm256_cmp_pd(mm_b, mm_rgb_xyz_cont[4], _CMP_GT_OQ));
	// 	auto mm_x = _mm256_mul_pd(mm_r, mm_rgb_xyz_mat[0]);
	// 	mm_x = _mm256_fmadd_pd(mm_g, mm_rgb_xyz_mat[1], mm_x);
	// 	mm_x = _mm256_fmadd_pd(mm_b, mm_rgb_xyz_mat[2], mm_x);
	// 	mm_x = _mm256_div_pd(mm_x, mm_xyz_lab_cont[0]);
	// 	auto mm_y = _mm256_mul_pd(mm_r, mm_rgb_xyz_mat[3]);
	// 	mm_y = _mm256_fmadd_pd(mm_g, mm_rgb_xyz_mat[4], mm_y);
	// 	mm_y = _mm256_fmadd_pd(mm_b, mm_rgb_xyz_mat[5], mm_y);
	// 	mm_y = _mm256_div_pd(mm_y, mm_xyz_lab_cont[1]);
	// 	auto mm_z = _mm256_mul_pd(mm_r, mm_rgb_xyz_mat[6]);
	// 	mm_z = _mm256_fmadd_pd(mm_g, mm_rgb_xyz_mat[7], mm_z);
	// 	mm_z = _mm256_fmadd_pd(mm_b, mm_rgb_xyz_mat[8], mm_z);
	// 	mm_z = _mm256_div_pd(mm_z, mm_xyz_lab_cont[2]);

	// 	mm_x = _mm256_blendv_pd(_ZGVdN4vv_pow(mm_x, mm_xyz_lab_cont[5]),
	// 							_mm256_fmadd_pd(mm_x, mm_xyz_lab_cont[4], mm_xyz_lab_cont[6]),
	// 							_mm256_cmp_pd(mm_x, mm_xyz_lab_cont[3], _CMP_LE_OQ));
	// 	mm_y = _mm256_blendv_pd(_ZGVdN4vv_pow(mm_y, mm_xyz_lab_cont[5]),
	// 							_mm256_fmadd_pd(mm_y, mm_xyz_lab_cont[4], mm_xyz_lab_cont[6]),
	// 							_mm256_cmp_pd(mm_y, mm_xyz_lab_cont[3], _CMP_LE_OQ));
	// 	mm_z = _mm256_blendv_pd(_ZGVdN4vv_pow(mm_z, mm_xyz_lab_cont[5]),
	// 							_mm256_fmadd_pd(mm_z, mm_xyz_lab_cont[4], mm_xyz_lab_cont[6]),
	// 							_mm256_cmp_pd(mm_z, mm_xyz_lab_cont[3], _CMP_LE_OQ));

	// 	_mm256_store_pd(lvec+j, _mm256_fmsub_pd(mm_y, mm_xyz_lab_cont[7], mm_xyz_lab_cont[8]));
	// 	_mm256_store_pd(avec+j, _mm256_mul_pd(_mm256_sub_pd(mm_x, mm_y), mm_xyz_lab_cont[9]));
	// 	_mm256_store_pd(bvec+j, _mm256_mul_pd(_mm256_sub_pd(mm_y, mm_z), mm_xyz_lab_cont[10]));
	// }
	// printf("%lf %lf %lf %lf %lf\n", count_time1, count_time2, count_time3, count_time4, 
	// 	(count_time1 + count_time2 + count_time3 + count_time4) / 1000);

	#pragma omp parallel for
	for(int j=j1; j < sz; j++ )
	{
		int r = (ubuff[j] >> 16) & 0xFF;
		int g = (ubuff[j] >>  8) & 0xFF;
		int b = (ubuff[j]      ) & 0xFF;
		// printf("%d %d %d\n", r, g, b);
		RGB2LAB( r, g, b, lvec[j], avec[j], bvec[j] );
	}
}

//==============================================================================
///	DetectLabEdges
/// 在 LAB 空间做边缘检测，假设要检测的点为 vec[x][y]，该算法就是计算
/// (vec[x-1][y] - vec[x+1][y])^2 + (vec[x][y-1] - vec[x][y+1])^2
//==============================================================================
void SLIC::DetectLabEdges(
	const double*				lvec,
	const double*				avec,
	const double*				bvec,
	const int&					width,
	const int&					height,
	vector<double>&				edges)
{
	int sz = width*height;

	edges.resize(sz,0);
	#pragma omp parallel for num_threads(4)
	for( int j = 1; j < height-1; j++ )
	{
		for( int k = 1; k < width-1; k++ )
		{
			int i = j*width+k;

			double dx = (lvec[i-1]-lvec[i+1])*(lvec[i-1]-lvec[i+1]) +
						(avec[i-1]-avec[i+1])*(avec[i-1]-avec[i+1]) +
						(bvec[i-1]-bvec[i+1])*(bvec[i-1]-bvec[i+1]);

			double dy = (lvec[i-width]-lvec[i+width])*(lvec[i-width]-lvec[i+width]) +
						(avec[i-width]-avec[i+width])*(avec[i-width]-avec[i+width]) +
						(bvec[i-width]-bvec[i+width])*(bvec[i-width]-bvec[i+width]);

			//edges[i] = (sqrt(dx) + sqrt(dy));
			edges[i] = (dx + dy);
		}
	}
}

//===========================================================================
///	PerturbSeeds
//===========================================================================
void SLIC::PerturbSeeds(
	vector<double>&				kseedsl,
	vector<double>&				kseedsa,
	vector<double>&				kseedsb,
	vector<double>&				kseedsx,
	vector<double>&				kseedsy,
	const vector<double>&		edges)
{
	const int dx8[8] = {-1, -1,  0,  1, 1, 1, 0, -1};	// 四周八个方向
	const int dy8[8] = { 0, -1, -1, -1, 0, 1, 1,  1};
	
	int numseeds = kseedsl.size();

	for( int n = 0; n < numseeds; n++ )
	{
		int ox = kseedsx[n];//original x
		int oy = kseedsy[n];//original y
		int oind = oy*m_width + ox;

		int storeind = oind;
		for( int i = 0; i < 8; i++ )
		{
			int nx = ox+dx8[i];//new x
			int ny = oy+dy8[i];//new y

			if( nx >= 0 && nx < m_width && ny >= 0 && ny < m_height)
			{
				int nind = ny*m_width + nx;
				if( edges[nind] < edges[storeind])
				{
					storeind = nind;
				}
			}
		}
		if(storeind != oind)
		{
			kseedsx[n] = storeind%m_width;
			kseedsy[n] = storeind/m_width;
			kseedsl[n] = m_lvec[storeind];
			kseedsa[n] = m_avec[storeind];
			kseedsb[n] = m_bvec[storeind];
		}
	}
}

//===========================================================================
///	GetLABXYSeeds_ForGivenK
///
/// The k seed values are taken as uniform spatial pixel samples.
//===========================================================================
void SLIC::GetLABXYSeeds_ForGivenK(
	vector<double>&				kseedsl,
	vector<double>&				kseedsa,
	vector<double>&				kseedsb,
	vector<double>&				kseedsx,
	vector<double>&				kseedsy,
	const int&					K,
	const bool&					perturbseeds,
	const vector<double>&		edgemag)
{
	int sz = m_width*m_height;
	double step = sqrt(double(sz)/double(K));
	int T = step;
	int xoff = step/2;
	int yoff = step/2;
	
	int n(0);int r(0);
	for( int y = 0; y < m_height; y++ )
	{
		int Y = y*step + yoff;
		if( Y > m_height-1 ) break;

		for( int x = 0; x < m_width; x++ )
		{
			//int X = x*step + xoff;//square grid
			int X = x*step + (xoff<<(r&0x1));//hex grid
			if(X > m_width-1) break;

			int i = Y*m_width + X;

			//_ASSERT(n < K);
			
			//kseedsl[n] = m_lvec[i];
			//kseedsa[n] = m_avec[i];
			//kseedsb[n] = m_bvec[i];
			//kseedsx[n] = X;
			//kseedsy[n] = Y;
			kseedsl.push_back(m_lvec[i]);
			kseedsa.push_back(m_avec[i]);
			kseedsb.push_back(m_bvec[i]);
			kseedsx.push_back(X);
			kseedsy.push_back(Y);
			n++;
		}
		r++;
	}

	if(perturbseeds)
	{
		PerturbSeeds(kseedsl, kseedsa, kseedsb, kseedsx, kseedsy, edgemag);
	}
}

//===========================================================================
///	PerformSuperpixelSegmentation_VariableSandM
///
///	Magic SLIC - no parameters
///
///	Performs k mean segmentation. It is fast because it looks locally, not
/// over the entire image.
/// This function picks the maximum value of color distance as compact factor
/// M and maximum pixel distance as grid step size S from each cluster (13 April 2011).
/// So no need to input a constant value of M and S. There are two clear
/// advantages:
///
/// [1] The algorithm now better handles both textured and non-textured regions
/// [2] There is not need to set any parameters!!!
///
/// SLICO (or SLIC Zero) dynamically varies only the compactness factor S,
/// not the step size S.
//===========================================================================
void SLIC::PerformSuperpixelSegmentation_VariableSandM(
	vector<double>&				kseedsl,
	vector<double>&				kseedsa,
	vector<double>&				kseedsb,
	vector<double>&				kseedsx,
	vector<double>&				kseedsy,
	int*						klabels,
	const int&					STEP,
	const int&					NUMITR)
{
	int sz = m_width*m_height;
	const int numk = kseedsl.size();
	//double cumerr(99999.9);
	int numitr(0);

	//----------------
	int offset = STEP;
	if(STEP < 10) offset = STEP*1.5;
	//----------------

	double* m_labz = align_new<double>(sz*4, 32);
	double* kseedslabz = align_new<double>(numk*4, 32);
	double* sigmalab = align_new<double>(numk*4, 32);

	for(int i=0; i<sz; i++){
		int index = i*4;
		m_labz[index] = m_lvec[i];
		m_labz[index+1] = m_avec[i];
		m_labz[index+2] = m_bvec[i];
		m_labz[index+4] = 0;
	}
	for(int i=0; i<numk; i++){
		int index = i*4;
		kseedslabz[index] = kseedsl[i];
		kseedslabz[index+1] = kseedsa[i];
		kseedslabz[index+2] = kseedsb[i];
		kseedslabz[index+3] = 0;
	}

	double* sigmal = align_new<double>(numk, 32);
	double* sigmaa = align_new<double>(numk, 32);
	double* sigmab = align_new<double>(numk, 32);
	double* sigmax = align_new<double>(numk, 32);
	double* sigmay = align_new<double>(numk, 32);
	int* clustersize = align_new<int>(numk, 32);
	
	double* sigmass[THE_THREAD_NUMS_SIGMA][5];
	int* clustersizess[THE_THREAD_NUMS_SIGMA];
	for(int i=0; i<THE_THREAD_NUMS_SIGMA; i++){
		clustersizess[i] = align_new<int>(numk, 32);
		for(int j=0; j<5; j++){
			sigmass[i][j] = align_new<double>(numk, 32);
		}
	}
	double* maxlab_p[THE_THREAD_NUMS_DIST];
	for(int i=0; i<THE_THREAD_NUMS_DIST; i++){
		maxlab_p[i] = align_new<double>(numk, 32);
		for(int j=0; j<numk; j++){
			maxlab_p[i][j] = 10*10;
		}
	}

	vector<double> inv(numk, 0);//to store 1/clustersize[k] values

	// double* distxy = align_new<double>(sz, 32);
	double* distlab = align_new<double>(sz, 32);
	// memset(distxy, CHAR_DOUBLE_AMAX, sz*sizeof(double));
	memset(distlab, CHAR_DOUBLE_AMAX, sz*sizeof(double));

	vector<double> maxlab(numk, 10*10);//THIS IS THE VARIABLE VALUE OF M, just start with 10
	vector<double> maxlab_n(numk, 10*10);//THIS IS THE VARIABLE VALUE OF M, just start with 10
	// vector<double> maxxy(numk, STEP*STEP);//THIS IS THE VARIABLE VALUE OF M, just start with 10

	double invxywt = 1.0/(STEP*STEP);//NOTE: this is different from how usual SLIC/LKM works
	double countTime = 0;
	double countTime2 = 0;
	
	double* distvec_y = align_new<double>(m_width, 32);
	while( numitr < NUMITR )	// Default: 10
	{
		//------
		//cumerr = 0;
		numitr++;
		//------
		auto tt1 = Clock::now();
		for(int i=0; i<THE_THREAD_NUMS_SIGMA; i++){
			memset(clustersizess[i], 0, numk*sizeof(int));
			for(int j=0; j<5; j++){
				memset(sigmass[i][j], 0, numk*sizeof(double));
			}
		}
		memset(sigmal, 0, numk*sizeof(double));
		memset(sigmaa, 0, numk*sizeof(double));
		memset(sigmab, 0, numk*sizeof(double));
		memset(sigmax, 0, numk*sizeof(double));
		memset(sigmay, 0, numk*sizeof(double));
		memset(clustersize, 0, numk*sizeof(int));
		auto tt2 = Clock::now();
    	auto compTime5 = chrono::duration_cast<chrono::microseconds>(tt2 - tt1);
    	// std::cout <<  "Dist iter time=" << compTime.count()/1000 << " ms " << std::endl;
		count_time5 += compTime5.count()/1000;
		std::cout <<  "init time=" << count_time5 << "(" << compTime5.count()/1000 << ") ms \t";


		auto startTime = Clock::now();
		if(only_once2) {cout << "numk = " << numk << endl; only_once2--;}
		#pragma omp parallel for num_threads(THE_THREAD_NUMS_DIST)// 我实在想不清楚为什么并行效果这么差, 内存带宽不够了吗
		for(int y = 0; y<m_height; y++){
			// double distvec_y[m_width+32];	// 这和我预期不太一样啊，为什么栈上空间比堆上空间慢了一倍
			double* distvec_y = align_new<double>(m_width, 32);
			double* distlab_y = align_new<double>(m_width, 32);
			memset(distvec_y, CHAR_DOUBLE_AMAX, m_width*sizeof(double));
			// #ifdef _OPENMP
			// printf(" (%d-%d) ", y, omp_get_thread_num());
			// #endif
			for(int n=0; n<numk; n++){
				if((int)(kseedsy[n]-offset) <= y && y < (int)(kseedsy[n]+offset)){
					const int x1 = max(0,		(int)(kseedsx[n]-offset));
					const int x2 = min(m_width,	(int)(kseedsx[n]+offset));
					const double tmp_kseedsl = kseedsl[n];
					const double tmp_kseedsa = kseedsa[n];
					const double tmp_kseedsb = kseedsb[n];
					const double tmp_kseedsx = kseedsx[n];
					const double tmp_kseedsy = kseedsy[n];
					// const double tmp_maxlab  = maxlab[n] * invxywt ;
					const double tmp_maxlab_r  = (STEP*STEP) / maxlab[n];
					// const double tmp_maxlab_r  = (STEP*STEP) / (double)(10*10);
					const int yindex = y * m_width;
					const double powery = (y - tmp_kseedsy) * (y - tmp_kseedsy);
					double tmpxy = (x1 - tmp_kseedsx)*(x1 - tmp_kseedsx) + powery - (x1*2 - tmp_kseedsx*2 - 1);
					// #pragma simd
					for(int x=x1; x<x2; x++){
						int i = yindex + x;
						distlab_y[x] =	(m_lvec[i] - tmp_kseedsl)*(m_lvec[i] - tmp_kseedsl) +
										(m_avec[i] - tmp_kseedsa)*(m_avec[i] - tmp_kseedsa) +
										(m_bvec[i] - tmp_kseedsb)*(m_bvec[i] - tmp_kseedsb);
					}
					for( int x = x1; x < x2; x++){
						int i = yindex + x;
						tmpxy += (x - tmp_kseedsx)*2 - 1;
						double dist = distlab_y[x] * tmp_maxlab_r + tmpxy;
						if( dist < distvec_y[x] ){
							distvec_y[x] = dist;
							klabels[i]  = n;
						}
					}
				}
			}
			const int thread_num = omp_get_thread_num();
			const int yindex = y * m_width;
			for (int x = 0; x < m_width; x++) {
			  	const int index = (yindex + x);
			  	const int kl = klabels[index];
				if(maxlab_p[thread_num][kl] < distlab_y[x]) maxlab_p[thread_num][kl] = distlab_y[x];
				// if(maxlab_n[kl] < distlab_y[x]) maxlab_n[kl] = distlab_y[x];
  		    	sigmass[thread_num][0][kl] += m_lvec[index];
  		    	sigmass[thread_num][1][kl] += m_avec[index];
  		    	sigmass[thread_num][2][kl] += m_bvec[index];
  		    	sigmass[thread_num][3][kl] += x;
  		    	sigmass[thread_num][4][kl] += y;
	
  		    	clustersizess[thread_num][kl]++;
			}
			align_delete(distvec_y);
			align_delete(distlab_y);
		}
		// memcpy(&maxlab[0], &maxlab_n[0], numk*sizeof(double));
		auto endTime = Clock::now();
    	auto compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
    	// std::cout <<  "Dist iter time=" << compTime.count()/1000 << " ms " << std::endl;
		countTime += compTime.count()/1000;
		std::cout <<  "Dist iter time=" << countTime << "(" << compTime.count()/1000 << ") ms \t";
		//-----------------------------------------------------------------
		// Assign the max color distance for a cluster
		//-----------------------------------------------------------------
		// if(0 == numitr)
		// {
		// 	maxlab.assign(numk,1);
		// 	maxxy.assign(numk,1);
		// }

		// memset(sigmalab, 0, numk*4*sizeof(double));

		startTime = Clock::now();
		for(int i=0; i<THE_THREAD_NUMS_DIST; i++){
			for(int j=0; j<numk; j++){
				maxlab[j] = maxlab[j] >= maxlab_p[i][j] ? maxlab[j] : maxlab_p[i][j];
			}
		}
		for(int i=0; i<THE_THREAD_NUMS_DIST; i++){
			memcpy(maxlab_p[i], &maxlab[0], numk*sizeof(double));
		}
		// {for( int i = 0; i < sz; i++ )
		// {
		// 	if(maxlab[klabels[i]] < distlab[i]) maxlab[klabels[i]] = distlab[i];
		// 	// if(maxxy[klabels[i]] < distxy[i]) maxxy[klabels[i]] = distxy[i];
		// }}
		// #pragma omp parallel for num_threads(THE_THREAD_NUMS_SIGMA)
  		// for (int y = 0; y < m_height; y++) {
		//   const int thread_num = omp_get_thread_num();
  		//   const int yindex = y * m_width;
  		//   for (int x = 0; x < m_width; x++) {
  		//     const int index = (yindex + x);
  		//     const int kl = klabels[index];
		// 	// const int index4 = index * 4;
		// 	// const int kl4 = kl*4;
		// 	// if(maxlab[kl] < distlab[index]) maxlab[kl] = distlab[index];
		// 	// if(maxxy[kl] < distxy[index]) maxxy[kl] = distxy[index];
		// 	// sigmalab[kl4] += m_labz[index4];
		// 	// sigmalab[kl4+1] += m_labz[index4+1];
		// 	// sigmalab[kl4+2] += m_labz[index4+2];
  		//     sigmass[thread_num][0][kl] += m_lvec[index];
  		//     sigmass[thread_num][1][kl] += m_avec[index];
  		//     sigmass[thread_num][2][kl] += m_bvec[index];
  		//     sigmass[thread_num][3][kl] += x;
  		//     sigmass[thread_num][4][kl] += y;

  		//     clustersizess[thread_num][kl]++;
  		//   }
  		// }
		// #pragma omp parallel for num_threads(THE_THREAD_NUMS_SIGMA/2)
		// for(int i=0; i<THE_THREAD_NUMS_SIGMA; i+=2){
		// 	for(int j=0; j<numk; j++){
  		//     	sigmass[i][0][j] += sigmass[i+1][0][j];
  		//     	sigmass[i][1][j] += sigmass[i+1][1][j];
  		//     	sigmass[i][2][j] += sigmass[i+1][2][j];
  		//     	sigmass[i][3][j] += sigmass[i+1][3][j];
  		//     	sigmass[i][4][j] += sigmass[i+1][4][j];

  		//     	clustersizess[i][j] += clustersizess[i+1][j];

		// 	}
		// }
		for(int i=0; i<THE_THREAD_NUMS_SIGMA; i++){
			for(int j=0; j<numk; j++){
  		    	sigmal[j] += sigmass[i][0][j];
  		    	sigmaa[j] += sigmass[i][1][j];
  		    	sigmab[j] += sigmass[i][2][j];
  		    	sigmax[j] += sigmass[i][3][j];
  		    	sigmay[j] += sigmass[i][4][j];

  		    	clustersize[j] += clustersizess[i][j];

			}
		}
		// 改动一：做了个循环合并，有一些效果
		// 改动二：把 sz 循环改成了 y, x 循环，去掉了求模和除法，效果不错
		// 失败尝试一：把 lab 合并，变慢，使用数组的结构而不是结构的数组真合理
		// 失败尝试二：难以并行，左边大小 numk，右边大小 sz，求和型，访存冲突严重
		// 失败尝试三：分块，原以为分块可以使得 klabels[index] 近似相同，从而访问同一片内存
  		// for (int y = 0; y < m_height; y++) {
  		//   const int yindex = y * m_width;
  		//   for (int x = 0; x < m_width; x++) {
  		//     const int index = (yindex + x);
  		//     const int kl = klabels[index];
		// 	// const int index4 = index * 4;
		// 	// const int kl4 = kl*4;
		// 	// if(maxlab[kl] < distlab[index]) maxlab[kl] = distlab[index];
		// 	// if(maxxy[kl] < distxy[index]) maxxy[kl] = distxy[index];
		// 	// sigmalab[kl4] += m_labz[index4];
		// 	// sigmalab[kl4+1] += m_labz[index4+1];
		// 	// sigmalab[kl4+2] += m_labz[index4+2];

  		//     sigmal[kl] += m_lvec[index];
  		//     sigmaa[kl] += m_avec[index];
  		//     sigmab[kl] += m_bvec[index];
  		//     sigmax[kl] += x;
  		//     sigmay[kl] += y;

  		//     clustersize[kl]++;
  		//   }
  		// }
		// update_sigma(sigmal, sigmaa, sigmab, sigmax, sigmay, clustersize, klabels, m_lvec, m_avec, m_bvec, sz, m_width, m_height, sigmalab, m_labz);

		// 下面这两段由于 numk 较小，时间可忽略不计，没有改动必要
		{for( int k = 0; k < numk; k++ )
		{
			//_ASSERT(clustersize[k] > 0);
			if( clustersize[k] <= 0 ) clustersize[k] = 1;
			inv[k] = 1.0/double(clustersize[k]);//computing inverse now to multiply, than divide later
		}}
		{for( int k = 0; k < numk; k++ )
		{
			// kseedsl[k] = sigmalab[k*4]*inv[k];
			// kseedsa[k] = sigmalab[k*4+1]*inv[k];
			// kseedsb[k] = sigmalab[k*4+2]*inv[k];
			kseedsl[k] = sigmal[k]*inv[k];
			kseedsa[k] = sigmaa[k]*inv[k];
			kseedsb[k] = sigmab[k]*inv[k];
			kseedsx[k] = sigmax[k]*inv[k];
			kseedsy[k] = sigmay[k]*inv[k];
		}}
		// for(int i=0; i<numk; i++){
		// 	int index = i*4;
		// 	kseedslabz[index] = kseedsl[i];
		// 	kseedslabz[index+1] = kseedsa[i];
		// 	kseedslabz[index+2] = kseedsb[i];
		// 	kseedslabz[index+3] = 0;
		// }
		endTime = Clock::now();
		compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
    	// std::cout <<  "Dist iter time=" << compTime.count()/1000 << " ms " << std::endl;
		countTime2 += compTime.count()/1000;
		std::cout <<  "Dist iter time=" << countTime2 << "(" << compTime.count()/1000 << ") ms " << std::endl;
	}
	
	align_delete(m_labz);
	align_delete(clustersize);
	align_delete(sigmal);
	align_delete(sigmaa);
	align_delete(sigmab);
	align_delete(sigmax);
	align_delete(sigmay);
	// align_delete(distxy);
	align_delete(distlab);
	for(int i=0; i<THE_THREAD_NUMS_SIGMA; i++){
		clustersizess[i] = align_new<int>(numk, 32);
		for(int j=0; j<5; j++){
			align_delete(sigmass[i][j]);
		}
	}
	align_delete(distvec_y);
	for(int i=0; i<THE_THREAD_NUMS_DIST; i++){
		align_delete(maxlab_p[i]);
	}

}

//===========================================================================
///	SaveSuperpixelLabels2PGM
///
///	Save labels to PGM in raster scan order.
//===========================================================================
void SLIC::SaveSuperpixelLabels2PPM(
	char*                           filename, 
	int *                           labels, 
	const int                       width, 
	const int                       height)
{
    FILE* fp;
    char header[20];
 
    fp = fopen(filename, "wb");
 
    // write the PPM header info, such as type, width, height and maximum
    fprintf(fp,"P6\n%d %d\n255\n", width, height);
 
    // write the RGB data
    unsigned char *rgb = new unsigned char [ (width)*(height)*3 ];
    int k = 0;
	unsigned char c = 0;
    for ( int i = 0; i < (height); i++ ) {
        for ( int j = 0; j < (width); j++ ) {
			c = (unsigned char)(labels[k]);
            rgb[i*(width)*3 + j*3 + 2] = labels[k] >> 16 & 0xff;  // r
            rgb[i*(width)*3 + j*3 + 1] = labels[k] >> 8  & 0xff;  // g
            rgb[i*(width)*3 + j*3 + 0] = labels[k]       & 0xff;  // b

			// rgb[i*(width) + j + 0] = c;
            k++;
        }
    }
    fwrite(rgb, width*height*3, 1, fp);

    delete [] rgb;
 
    fclose(fp);

}

//===========================================================================
///	EnforceLabelConnectivity
///
///		1. finding an adjacent label for each new component at the start
///		2. if a certain component is too small, assigning the previously found
///		    adjacent label to this component, and not incrementing the label.
//===========================================================================
void SLIC::EnforceLabelConnectivity(
	const int*					labels,//input labels that need to be corrected to remove stray labels
	const int&					width,
	const int&					height,
	int*						nlabels,//new labels
	int&						numlabels,//the number of labels changes in the end if segments are removed
	const int&					K) //the number of superpixels desired by the user
{
//	const int dx8[8] = {-1, -1,  0,  1, 1, 1, 0, -1};
//	const int dy8[8] = { 0, -1, -1, -1, 0, 1, 1,  1};

	const int dx4[4] = {-1,  0,  1,  0};
	const int dy4[4] = { 0, -1,  0,  1};

	const int sz = width*height;
	const int SUPSZ = sz/K;
	//nlabels.resize(sz, -1);

	// for( int i = 0; i < sz; i++ ) nlabels[i] = -1;
	
	// int label(0);
	atomic_int label(0);
	int* xvec = new int[sz];
	int* yvec = new int[sz];
	int oindex(0);
	int adjlabel(0);//adjacent label
	
	int** xvec_p = new int*[THE_THREAD_NUMS_ENFORCE];
	int** yvec_p = new int*[THE_THREAD_NUMS_ENFORCE];
	for(int i=0; i<THE_THREAD_NUMS_ENFORCE; i++){
		xvec_p[i] = new int[sz / THE_THREAD_NUMS_ENFORCE + 1];
		yvec_p[i] = new int[sz / THE_THREAD_NUMS_ENFORCE + 1];
	}
	int* labels_size = new int[sz];
	int* labels_ivec = new int[sz];
	int* nnlabels = new int[sz];
	memset(labels_size, 0, sizeof(int)*sz);
	int* labels_map = new int[sz];
	for(int i=0; i<sz; i++){
		labels_map[i] = i;
	}
	int pheight = height;
	// vector<
	// 1、确认连通性，多线程，合并
	// 2、踢掉小块的
	// 3、重新遍历，按照顺序重写 label
	#pragma omp parallel num_threads(THE_THREAD_NUMS_ENFORCE)
	{
	// #pragma omp parallel for num_threads(THE_THREAD_NUMS_ENFORCE) schedule(static, 1)
	#pragma omp for schedule(static, 1)
	for(int nt=0; nt<THE_THREAD_NUMS_ENFORCE; nt++){
		const int thread_num = omp_get_thread_num();
		int ymin = (height / THE_THREAD_NUMS_ENFORCE) * thread_num;
		int ymax = (height / THE_THREAD_NUMS_ENFORCE) * (thread_num+1);
		if(thread_num == THE_THREAD_NUMS_ENFORCE-1){
			pheight = ymax;
			ymax = height;
		}
		memset(nlabels+(ymin*width), -1, sizeof(int) * (ymax - ymin) * m_width);
		// printf("(%4d, %4d, %2d)\t", ymin, ymax, thread_num);
		for(int y = ymin; y<ymax; y++){
			const int yindex = y*width;
			for(int x=0; x<width; x++){
				int oindex = yindex + x;
				if(0 > nlabels[oindex] ){
					int tmp_label = label++;
					// int tmp_label = getLabel();
					nlabels[oindex] = tmp_label;
					xvec_p[thread_num][0] = x;
					yvec_p[thread_num][0] = y;
					int count(1);
					for( int c = 0; c < count; c++ ){
						for( int n = 0; n < 4; n++ ){
							int x = xvec_p[thread_num][c] + dx4[n];
							int y = yvec_p[thread_num][c] + dy4[n];
							if( (x >= 0 && x < width) && (y >= ymin && y < ymax) ){
								int nindex = y*width + x;
								if( 0 > nlabels[nindex] && labels[oindex] == labels[nindex] ){
									xvec_p[thread_num][count] = x;
									yvec_p[thread_num][count] = y;
									nlabels[nindex] = tmp_label;
									count++;
								}
							}
						}
					}
					labels_size[tmp_label] = count;
					// labels_ivec[tmp_label] = oindex;
				}
			}
		}
	}
	#pragma omp for
	for(int y=(height / THE_THREAD_NUMS_ENFORCE); y<pheight; y+=(height / THE_THREAD_NUMS_ENFORCE)){
		const int yindex = y * width;
		for(int x=0; x<width; x++){
			const int index = yindex + x;
			if(labels[index - width] == labels[index] 
			&& labels_map[nlabels[index]] != labels_map[nlabels[index - width]]){
				if(labels_ivec[union_find(labels_map, nlabels[index - width])]
				>= labels_ivec[union_find(labels_map, nlabels[index])])
					labels_map[labels_map[nlabels[index - width]]] = labels_map[nlabels[index]];
				else 
					labels_map[labels_map[nlabels[index]]] = labels_map[nlabels[index - width]];
			}
		}
	}
	}
	// cout << endl;
	// #pragma omp parallel for num_threads(THE_THREAD_NUMS_ENFORCE)
	// for(int y=(height / THE_THREAD_NUMS_ENFORCE); y<pheight; y+=(height / THE_THREAD_NUMS_ENFORCE)){
	// 	// printf("(%d)\t", y);
	// 	const int yindex = y * width;
	// 	for(int x=0; x<width; x++){
	// 		const int index = yindex + x;
	// 		if(labels[index - width] == labels[index] 
	// 		&& labels_map[nlabels[index]] != labels_map[nlabels[index - width]]){
	// 			if(labels_ivec[union_find(labels_map, nlabels[index - width])]
	// 			>= labels_ivec[union_find(labels_map, nlabels[index])])
	// 				labels_map[labels_map[nlabels[index - width]]] = labels_map[nlabels[index]];
	// 			else 
	// 				labels_map[labels_map[nlabels[index]]] = labels_map[nlabels[index - width]];
	// 		}
	// 	}
	// }
	for(int i=0; i<label; i++){
		union_find(labels_map, i);
		if(labels_map[i] != i){
			labels_size[labels_map[i]] += labels_size[i];
		}
	}
	// adjlabel = labels_map[nlabels[0]];
	int* labels_map2 = new int[label];
	memset(labels_map2, -1, sizeof(int)*label);
	int clabel(0);
	for(int y=0; y<height; y++){
		const int yindex = y*width;
		for(int x=0; x<width; x++){
			const int index = yindex + x;
			int local_label = labels_map[nlabels[index]];
			if(labels_map2[local_label] == -1){
				if(labels_size[local_label] <= SUPSZ >> 2){
					for(int n=0; n<4; n++){
						int tx = x + dx4[n];
						int ty = y + dy4[n];
						if( (tx >= 0 && tx < width) && (ty >= 0 && ty < height) ){
							int nindex = ty*width + tx;
							if(labels_map2[labels_map[nlabels[nindex]]] != -1)
								adjlabel = labels_map[nlabels[nindex]];
						}
					}
					if(labels_map2[adjlabel] == -1) {
						labels_map2[local_label] = 0;
						// printf("\n!!!!!!!!!!!!\n");
					}
					else{
						labels_map2[local_label] = labels_map2[adjlabel];
					}
					// labels_map[nlabels[index]] = adjlabel;
					// local_label = adjlabel;
				}
				else{
					labels_map2[local_label] = clabel;
					clabel++;
				}
			}
			nnlabels[index] = labels_map2[local_label];
		}
	}
	// for(int y=0; y<height; y++){
	// 	printf("(%4d) ", y);
	// 	for(int x=0; x<width; x++){
	// 		printf("%3d ", labels_map[nlabels[y*width+x]]);
	// 		if(x%20==0) printf("\n");
	// 	}
	// 	printf("\n");
	// }
	memcpy(nlabels, nnlabels, sizeof(int)*sz);
	numlabels = clabel;
	if(labels_map) delete [] labels_map;
	if(labels_map2) delete [] labels_map2;
	if(labels_size) delete [] labels_size;
	if(labels_ivec) delete [] labels_ivec;
	if(nnlabels) delete [] nnlabels;

	// label = 0;

	// 下面的逻辑就是，每遇到一个没打标签的点，就把它广度优先搜索
	// 如果这片区域太小，就和别的合并
	// 并行难点在于，给每个区块打标签是按顺序的
	// for( int j = 0; j < height; j++ )
	// {
	// 	for( int k = 0; k < width; k++ )
	// 	{
	// 		if( 0 > nlabels[oindex] )
	// 		{
	// 			nlabels[oindex] = label;
	// 			//--------------------
	// 			// Start a new segment
	// 			//--------------------
	// 			xvec[0] = k;
	// 			yvec[0] = j;
	// 			//-------------------------------------------------------
	// 			// Quickly find an adjacent label for use later if needed
	// 			//-------------------------------------------------------
	// 			{for( int n = 0; n < 4; n++ )
	// 			{
	// 				int x = xvec[0] + dx4[n];
	// 				int y = yvec[0] + dy4[n];
	// 				if( (x >= 0 && x < width) && (y >= 0 && y < height) )
	// 				{
	// 					int nindex = y*width + x;
	// 					if(nlabels[nindex] >= 0) adjlabel = nlabels[nindex];
	// 				}
	// 			}}
	// 			// 广度优先搜索，深度优先并不能更好利用缓存
	// 			// 试试按照 y 分类？
	// 			// 以及，原来的广搜不会重复吗
	// 			int count(1);
	// 			for( int c = 0; c < count; c++ )
	// 			{
	// 				// if(some_times1){
	// 				// 	printf("(%d, %d)\t", xvec[c], yvec[c]);
	// 				// 	some_times1--;
	// 				// }
	// 				for( int n = 0; n < 4; n++ )
	// 				{
	// 					int x = xvec[c] + dx4[n];
	// 					int y = yvec[c] + dy4[n];
	// 					if( (x >= 0 && x < width) && (y >= 0 && y < height) )
	// 					{
	// 						int nindex = y*width + x;

	// 						if( 0 > nlabels[nindex] && labels[oindex] == labels[nindex] )
	// 						{
	// 							xvec[count] = x;
	// 							yvec[count] = y;
	// 							nlabels[nindex] = label;
	// 							count++;
	// 						}
	// 					}

	// 				}
	// 			}
	// 			// printf("%5d\t", count);
	// 			//-------------------------------------------------------
	// 			// If segment size is less then a limit, assign an
	// 			// adjacent label found before, and decrement label count.
	// 			//-------------------------------------------------------
	// 			if(count <= SUPSZ >> 2)
	// 			{
	// 				for( int c = 0; c < count; c++ )
	// 				{
	// 					int ind = yvec[c]*width+xvec[c];
	// 					nlabels[ind] = adjlabel;
	// 				}
	// 				label--;
	// 			}
	// 			label++;
	// 		}
	// 		oindex++;
	// 	}
	// }
	// numlabels = label;

	// for(int y=0; y<height; y++){
	// 	printf("(%4d) ", y);
	// 	for(int x=0; x<width; x++){
	// 		printf("%3d ", nlabels[y*width+x]);
	// 		if(x%20==0) printf("\n");
	// 	}
	// 	printf("\n");
	// }
	if(xvec) delete [] xvec;
	if(yvec) delete [] yvec;
}

//===========================================================================
///	PerformSLICO_ForGivenK
///
/// Zero parameter SLIC algorithm for a given number K of superpixels.
//===========================================================================
void SLIC::PerformSLICO_ForGivenK(
	const unsigned int*			ubuff,
	const int					width,
	const int					height,
	int*						klabels,
	int&						numlabels,
	const int&					K,//required number of superpixels
	const double&				m)//weight given to spatial distance
{
	vector<double> kseedsl(0);
	vector<double> kseedsa(0);
	vector<double> kseedsb(0);
	vector<double> kseedsx(0);
	vector<double> kseedsy(0);

	//--------------------------------------------------
	m_width  = width;
	m_height = height;
	int sz = m_width*m_height;

#ifdef __TEST__
auto startTime = Clock::now();
#endif
	//--------------------------------------------------
	//if(0 == klabels) klabels = new int[sz];
	// for( int s = 0; s < sz; s++ ) klabels[s] = -1;	// 初始化类别
	memset(klabels, -1, sz*sizeof(int));
	//--------------------------------------------------
#ifdef __TEST__
auto endTime = Clock::now();
auto compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
std::cout <<  "Initial time = " << compTime.count()/1000 << " ms" << std::endl;
startTime = Clock::now();
#endif
	if(1)//LAB
	{
		DoRGBtoLABConversion(ubuff, m_lvec, m_avec, m_bvec);
	}
	else//RGB
	{
		// m_lvec = new double[sz]; m_avec = new double[sz]; m_bvec = new double[sz];
		m_lvec = align_new<double>(sz, 32); m_avec = align_new<double>(sz, 32); m_bvec = align_new<double>(sz, 32);
		for( int i = 0; i < sz; i++ )
		{
			m_lvec[i] = ubuff[i] >> 16 & 0xff;
			m_avec[i] = ubuff[i] >>  8 & 0xff;
			m_bvec[i] = ubuff[i]       & 0xff;
		}
	}
#ifdef __TEST__
endTime = Clock::now();
compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
std::cout <<  "Conversion time = " << compTime.count()/1000 << " ms" << std::endl;
#endif	
	//--------------------------------------------------
#ifdef __TEST__
startTime = Clock::now();
#endif
	bool perturbseeds(true);
	vector<double> edgemag(0);
	if(perturbseeds) DetectLabEdges(m_lvec, m_avec, m_bvec, m_width, m_height, edgemag);
	GetLABXYSeeds_ForGivenK(kseedsl, kseedsa, kseedsb, kseedsx, kseedsy, K, perturbseeds, edgemag);
#ifdef __TEST__
endTime = Clock::now();
compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
std::cout <<  "DeleteEdges and Get_Seeds time = " << compTime.count()/1000 << " ms" << std::endl;
#endif

#ifdef __TEST__
startTime = Clock::now();
#endif
	int STEP = sqrt(double(sz)/double(K)) + 2.0;//adding a small value in the even the STEP size is too small.
	PerformSuperpixelSegmentation_VariableSandM(kseedsl,kseedsa,kseedsb,kseedsx,kseedsy,klabels,STEP,10);
	numlabels = kseedsl.size();
#ifdef __TEST__
endTime = Clock::now();
compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
std::cout <<  "STEP = " << STEP << std::endl;
std::cout <<  "Segmentation time = " << compTime.count()/1000 << " ms" << std::endl;
#endif

#ifdef __TEST__
startTime = Clock::now();
#endif
	int* nlabels = new int[sz];
	EnforceLabelConnectivity(klabels, m_width, m_height, nlabels, numlabels, K);
#ifdef __TEST__
endTime = Clock::now();
compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
std::cout <<  "EnforceLabelConnectivity time = " << compTime.count()/1000 << " ms" << std::endl;
#endif

	{for(int i = 0; i < sz; i++ ) klabels[i] = nlabels[i];}
	if(nlabels) delete [] nlabels;
}

//===========================================================================
/// Load PPM file
///
/// 
//===========================================================================
void LoadPPM(char* filename, unsigned int** data, int* width, int* height)
{
    char header[1024];
    FILE* fp = NULL;
    int line = 0;
 
    fp = fopen(filename, "rb");
 
    // read the image type, such as: P6
    // skip the comment lines
    while (line < 2) {    
        fgets(header, 1024, fp);
        if (header[0] != '#') {
            ++line;
        }
    }
    // read width and height
    sscanf(header,"%d %d\n", width, height);
 
    // read the maximum of pixels
    fgets(header, 20, fp);
 
    // get rgb data
    unsigned char *rgb = new unsigned char [ (*width)*(*height)*3 ];
    fread(rgb, (*width)*(*height)*3, 1, fp);

    // *data = new unsigned int [ (*width)*(*height)*4 ];
	*data = align_new<unsigned int>((*width)*(*height)*4, 32);
    int k = 0;
    for ( int i = 0; i < (*height); i++ ) {
        for ( int j = 0; j < (*width); j++ ) {
            unsigned char *p = rgb + i*(*width)*3 + j*3;
                                      // a ( skipped )
            (*data)[k]  = p[2] << 16; // r
            (*data)[k] |= p[1] << 8;  // g
            (*data)[k] |= p[0];       // b
            k++;
        }
    }
    // ofc, later, you'll have to cleanup
    delete [] rgb;
 
    fclose(fp);
}

//===========================================================================
/// Load PPM file
///
/// 
//===========================================================================
int CheckLabelswithPPM(char* filename, int* labels, int width, int height)
{
    char header[1024];
    FILE* fp = NULL;
    int line = 0, ground = 0;
 
    fp = fopen(filename, "rb");
 
    // read the image type, such as: P6
    // skip the comment lines
    while (line < 2) {    
        fgets(header, 1024, fp);
        if (header[0] != '#') {
            ++line;
        }
    }
    // read width and height
	int w(0);
	int h(0);
    sscanf(header,"%d %d\n", &w, &h);
	if (w != width || h != height) return -1;
 
    // read the maximum of pixels
    fgets(header, 20, fp);
 
    // get rgb data
    unsigned char *rgb = new unsigned char [ (w)*(h)*3 ];
    fread(rgb, (w)*(h)*3, 1, fp);

    int num = 0, k = 0;
    for ( int i = 0; i < (h); i++ ) {
        for ( int j = 0; j < (w); j++ ) {
            unsigned char *p = rgb + i*(w)*3 + j*3;
                                  // a ( skipped )
            ground  = p[2] << 16; // r
            ground |= p[1] << 8;  // g
            ground |= p[0];       // b
            
			if (ground != labels[k])
				num++;

			k++;
        }
    }

    // ofc, later, you'll have to cleanup
    delete [] rgb;
 
    fclose(fp);

	return num;
}

//===========================================================================
///	The main function
///
//===========================================================================
int main (int argc, char **argv)
{
	unsigned int* img = NULL;
	// unsigned char* irgb = NULL;
	// struct RGB img;
	// img.r = img.g = img.b = NULL;
	int width(0);
	int height(0);

	LoadPPM((char *)"input_image.ppm", &img, &width, &height);
	if (width == 0 || height == 0) return -1;

	int sz = width*height;
	#ifdef __TEST__
	std::cout << "width = " << width << ", height = " << height << std::endl;
	std::cout << "sz = " << sz << std::endl;
	#endif
	int* labels = new int[sz];
	int numlabels(0);
	SLIC slic;
	int m_spcount;
	double m_compactness;
	m_spcount = 200;
	m_compactness = 10.0;
    auto startTime = Clock::now();
	slic.PerformSLICO_ForGivenK(img, width, height, labels, numlabels, m_spcount, m_compactness);//for a given number K of superpixels
    auto endTime = Clock::now();
    auto compTime = chrono::duration_cast<chrono::microseconds>(endTime - startTime);
    std::cout <<  "Computing time=" << compTime.count()/1000 << " ms" << std::endl;

	int num = CheckLabelswithPPM((char *)"check.ppm", labels, width, height);
	if (num < 0) {
		std::cout <<  "The result for labels is different from output_labels.ppm." << std::endl;
	} else {
		std::cout <<  "There are " << num << " points' labels are different from original file." << std::endl;
	}
	
	slic.SaveSuperpixelLabels2PPM((char *)"output_labels.ppm", labels, width, height);
	if(labels) delete [] labels;
	
	// if(img) delete [] img;
	if(img) align_delete(img);

	return 0;
}
